#include "../lib/lgd.h"
#include "iostream"
#include "cmath"
#include "vector"
#include "string"
#include "sstream"
#include "fstream"

#define pi (atan(1.0)*4.0)

//方向梯度类型
enum gradient_type_e
{
	Dnull,
	Dx,
	Dy
};

//正态函数参数组
struct gaussian_para
{
	double mu_x, mu_y, sigma_x, sigma_y, rho;
};

class sample : public LGD_Solver
{
protected:
	// 声明我们所使用的二维高斯分布的参数
	std::vector<gaussian_para> para;

public:
	sample() : LGD_Solver(){}
	virtual ~sample(){}

	void add_gauss_para(const gaussian_para &tmp_gp)
	{
		para.push_back(tmp_gp);
	}

	double gaussian_distribution(double x, double y, const gaussian_para &p, gradient_type_e mode = Dnull)
	{
		double part = -0.5*(pow((x-p.mu_x)/p.sigma_x,2) -2*p.rho*(x-p.mu_x)*(y-p.mu_y)/(p.sigma_x*p.sigma_y)
			+ pow((y-p.mu_y)/p.sigma_y,2)) / (1.0-p.rho*p.rho);

		double part2;
		if (mode == Dnull)
		{
			return exp(part)/(2*pi*p.sigma_x*p.sigma_y*sqrt(1-p.rho*p.rho));
		}

		if (mode == Dx)
		{
			part2 = -1.0*((x-p.mu_x)/(p.sigma_x*p.sigma_x) - p.rho*(y-p.mu_y)/(p.sigma_x*p.sigma_y))/(1.0-p.rho*p.rho);
			return part2*exp(part)/(2*pi*p.sigma_x*p.sigma_y*sqrt(1-p.rho*p.rho));
		}

		if (mode == Dy)
		{
			part2 = -1.0*((y-p.mu_y)/(p.sigma_y*p.sigma_y) - p.rho*(x-p.mu_x)/(p.sigma_x*p.sigma_y))/(1.0-p.rho*p.rho);
			return part2*exp(part)/(2*pi*p.sigma_x*p.sigma_y*sqrt(1-p.rho*p.rho));
		}

		std::string err_str = "Invalid calculation mode.";
		throw err_str;
	}

	lgd_float Evaluate(const lgd_float *x, lgd_float *g, const int n, const int m)
	{
		lgd_float fx = 0.0;
		for (int i = 0; i < para.size(); i++)
		{
			fx += -1.0*gaussian_distribution(x[0], x[1], para[i]);
		}
		fx += 12.82906044;

		if (g != NULL)
		{
			g[0] = g[1] = 0.0;
			for (int i = 0; i < para.size(); i++)
			{
				g[0] += -1.0*gaussian_distribution(x[0], x[1], para[i], Dx);
				g[1] += -1.0*gaussian_distribution(x[0], x[1], para[i], Dy);
			}
			g[0] *= 2.0*fx;
			g[1] *= 2.0*fx;
		}
		return fx*fx;
	}
};

int main(int argc, char *argv[])
{
	sample se;

	gaussian_para tmp_gp;
	tmp_gp.mu_x = 0.50; tmp_gp.mu_y = 0.40; tmp_gp.sigma_x = 0.15; tmp_gp.sigma_y = 0.10; tmp_gp.rho = 0.2;
	se.add_gauss_para(tmp_gp);

	tmp_gp.mu_x = 0.60; tmp_gp.mu_y = 0.70; tmp_gp.sigma_x = 0.10; tmp_gp.sigma_y = 0.20; tmp_gp.rho = 0.0;
	se.add_gauss_para(tmp_gp);

	lgd_float fx;
	lgd_float x[2] = {0.25, 0.20};
	lgd_float low[2] = {0.0, 0.0};
	lgd_float hig[2] = {1.0, 1.0};

	lgd_para my_para = lgd_default_parameters();
	my_para.flight_times = 2000;
	se.set_lgd_parameter(my_para);

	lgd_float x_mean[2] = {0.0, 0.0};
	lgd_float x_stddev[2] = {0.0, 0.0};

	se.Minimize(nullptr, &x_mean[0], &x_stddev[0], &fx, &x[0], 2, &low[0], &hig[0]);

	std::clog << "best-fx = " << fx << std::endl;
	std::clog << "best-model: " << x[0] << " " << x[1] << std::endl;
	std::clog << "mean-model: " << x_mean[0] << " " << x_mean[1] << std::endl;
	std::clog << "mean-stddev: " << x_stddev[0] << " " << x_stddev[1] << std::endl;
	std::clog << "x-range: " << x_mean[0] - x_stddev[0] << " " << x_mean[0] + x_stddev[0] << std::endl;
	std::clog << "y-range: " << x_mean[1] - x_stddev[1] << " " << x_mean[1] + x_stddev[1] << std::endl;
	return 0;
}